﻿using UnityEngine;
using System.Collections;
using System;


public class droneMovementController2 : MonoBehaviour {


    // Rotors of the drone (have to be associated to the four rotors of the drone, with the order V1,O1,V2,O2)
    public rotor helixV1;
    public rotor helixV2;
    public rotor helixO1;
    public rotor helixO2;

    void applyTorque(float amount) { transform.Rotate(transform.up, amount * Time.deltaTime); }

    float torqueGeneratedBy(rotor r) { return (r.counterclockwise ? -1 : 1) * denormalizeTorque(r.getPower()) * 10; }


    float denormalizePower(float pow) { return denormalize(pow, droneSettings.saturationValues.minRotationSpeed, droneSettings.saturationValues.maxRotationSpeed); }


    float denormalizeTorque(float pow) { return denormalize(pow, droneSettings.saturationValues.minTorque, droneSettings.saturationValues.maxTorque); }

    float denormalize(float pow, float lBound, float uBound) { return pow * (uBound - lBound) + lBound; }

    float keepOnRange01(float num) { return (float.IsNaN(num) ? 0 : droneSettings.keepOnRange(num, 0f, 1f)); }


    // variables where is stored, in a range [0,1], the power of each rotor
    public float pV1;
    public float pV2;
    public float pO1;
    public float pO2;


    private void modifyAllRotorsRotation(float intensity)
    {
        pV1 += intensity;
        pV2 += intensity;
        pO1 += intensity;
        pO2 += intensity;
    }
  
    private void modifyRollRotorsRotation(float intensity)
    {
        pV1 += intensity; pV2 -= intensity;
        pO2 += intensity; pO1 -= intensity;
    }


    private void modifyPitchRotorsRotation(float intensity)
    {
        pV1 += intensity; pV2 -= intensity;
        pO1 += intensity; pO2 -= intensity;
    }

    private void modifyPairsRotorsRotation(float intensity)
    {
        pV1 += intensity;
        pV2 += intensity;
        pO1 -= intensity;
        pO2 -= intensity;
    }

    // private void OnCollisionEnter(Collision collision)
    // {
    //     if (collision.collider.CompareTag("BadSphere"))
    //     {
    //         agent.AddReward(-1);
    //         if((bool)agent.envCtrler.epm.GetPara(EP.isCrashEndEpisode))
    //             agent.OnEpisodeEnd();
    //         //Debug.Log("You collided with the BadSphere.");
    //     }

    //     if(collision.collider.CompareTag("Target"))
    //     {
    //         agent.AddReward(1);
    //         agent.OnEpisodeEnd();
    //         //agent.gameObject.SetActive(false);

    //         Debug.Log("I Reached the target!");
    //     }
    // }

    void Update()
    {
        pO1 = 0f;
        pO2 = 0f;
        pV1 = 0f;
        pV2 = 0f;
        if(Input.GetKey(KeyCode.W))
            pO1 = 1f;
        if(Input.GetKey(KeyCode.S))
            pO2 = 1f;
        if(Input.GetKey(KeyCode.A))
            pV1 = 1f;
        if(Input.GetKey(KeyCode.D))
            pV2 = 1f;
        
    }

    void FixedUpdate()
    {
        pV1 = keepOnRange01(pV1);
        pV2 = keepOnRange01(pV2);
        pO1 = keepOnRange01(pO1);
        pO2 = keepOnRange01(pO2);
        helixV1.setPower(denormalizePower(pV1));
        helixV2.setPower(denormalizePower(pV2));
        helixO1.setPower(denormalizePower(pO1));
        helixO2.setPower(denormalizePower(pO2));
    }


}